package com.nighthawk;

import java.util.Arrays;
import java.util.Timer;
import java.util.TimerTask;
import java.util.Vector;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.util.Log;
import android.view.View;
import android.widget.Button;
import android.widget.TextView;

public class NightHawk extends Activity implements SensorEventListener{

	// this class will hold acceleration values as well as orientation stuff
	/*
	 * private class PositionClass {
		// rotation matrix maybe
		// accelerometer values
	}
	 */

	//Calculating Shit
	private Timer timeTimer;
	private boolean isChanged;
	private float[] vAvgVelocity = new float[3];
	private float[] vPosition = new float[3];
	private float[] vVelocity = new float[3];
	private float[] vAcceleration = new float[3];
	//private float[] calibrationOffset = new float[3];

	// Timer variables
	private long timeLastUpdate;
	private long timeCurrentTime;
	private float TIME_STEP;

	// Sensor variables
	private SensorManager sensorManager;
	private Sensor sensorAccelerometer;
	private Sensor sensorMagnetic;

	// Display variables
	private TextView tvXCoordinate;
	private TextView tvYCoordinate;
	private TextView tvZCoordinate;
//	private TextView tvXTotal;
	private TextView tvError;
//	private TextView tvAzimuth;	// angle between magnetic N direction and Y axis, around Z axis (0-359). 0=N, 90=E, 180=S, 270=W 
//	private TextView tvPitch; 	// rotation around X axis (-180-180), positive values when Z axis moves toward Y axis.  
//	private TextView tvRoll; 	// rotation around Y axis (-90 - 90), positive values when X axis moves toward Z axis
	private TextView tvDisplace; //displacement
	// roll angle positive in the clockwise direction

	// Orientation matrices
	private Vector<Float> accelerometerVector;
	private float[] accelerometerValues;
	private float[] orientationValues;
	private float[] rotationMatrix;
	private float[] inclinationMatrix;
	private float[] geomagneticMatrix;

	// Button stuff
	private Button btStartStop;
//	private Button btCalibrate;
	private boolean enabled;
	private boolean sensorReady;

	// Container variables
	/* May or may not be necessary, JIT compilation may work
	private ArrayList<PositionClass> arrayXValues;
	private ArrayList<PositionClass> arrayYValues;
	private ArrayList<PositionClass> arrayZValues;
	 */

	// Test variables
	private float floatXTotal;

	/** Called when the activity is first created. */
	public void onCreate(Bundle savedInstanceState) {

		// Program initialization
		super.onCreate(savedInstanceState);
		setContentView(R.layout.main);

		// Data initialization
		timeLastUpdate = -1;
		timeCurrentTime = 0;
		floatXTotal = 0.0f;
		enabled = false;

		//Timer shit
		timeTimer = new Timer();
		TIME_STEP = 0.1f;


		// Orientation initialization
		//accelerometerValues = new float[3];
		orientationValues = new float[3];
		rotationMatrix = new float[16];
		inclinationMatrix = new float[16];
		geomagneticMatrix = new float[3];

		sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);

		try {
			sensorAccelerometer = (Sensor) sensorManager.getSensorList(Sensor.TYPE_ACCELEROMETER).get(0); //accelerometer sensor
			sensorMagnetic = (Sensor) sensorManager.getSensorList(Sensor.TYPE_MAGNETIC_FIELD).get(0);
			sensorManager.registerListener(this, sensorAccelerometer, SensorManager.SENSOR_DELAY_NORMAL);
			sensorManager.registerListener(this, sensorMagnetic, SensorManager.SENSOR_DELAY_NORMAL);
		}catch(Exception e){
			//TODO you get a null pointer exception immediately if you use this, because it cannot find the button :\
			tvError = new TextView(this);
			tvError.setText("I can haz sensor?");
			setContentView(tvError);
		}

		tvXCoordinate = (TextView) findViewById(R.id.x_coordinate);
		tvYCoordinate = (TextView) findViewById(R.id.y_coordinate);
		tvZCoordinate = (TextView) findViewById(R.id.z_coordinate);
//		tvXTotal = (TextView) findViewById(R.id.x_total);
//		tvAzimuth = (TextView) findViewById(R.id.azimuth_value);
//		tvPitch = (TextView) findViewById(R.id.pitch_value);
//		tvRoll = (TextView) findViewById(R.id.roll_value);
		tvDisplace = (TextView) findViewById(R.id.displace_value);

		//This block of code looks soo confusing now lol
		btStartStop = (Button) findViewById(R.id.start_stop);
//		btCalibrate = (Button) findViewById(R.id.calibrate);

		btStartStop.setOnClickListener(new View.OnClickListener(){
			public void onClick(View v) {
				if(!enabled){
					vAvgVelocity = new float[3];
					vPosition = new float[3];
					vVelocity = new float[3];
					vAcceleration = new float[3];
					btStartStop.setText("Stop");
					timeTimer.scheduleAtFixedRate(new TimerTask(){
						@Override
						public void run() {
							calculateShit();
						}
					}, 0, (long) (TIME_STEP*1000)); //200 millisecond period in between executions
				} else {
					btStartStop.setText("Start");
					try{
						timeTimer.cancel(); //cancel the timer when the person wants to stop
						timeTimer.purge(); //purges the timer, just in case
						timeTimer = new Timer(); //maybe? doesn't hurt
					} catch (Exception e){

					}
				}
				enabled = !enabled;
			}
		});
		
//		btCalibrate.setOnClickListener(new View.OnClickListener(){
//			public void onClick(View v) {
//				calibrationOffset[0] = accelerometerValues[0];
//				calibrationOffset[1] = accelerometerValues[1];
//				calibrationOffset[2] = accelerometerValues[2];
//			}
//		});
	}

	public void calculateShit(){
		/* vAvgVelocity = vVelocity + vAcceleration * fDelta/2.0f;
		 * vPosition += vAvgVelocity * fDelta;
		 * vAcceleration = vSumOfForces*fDelta/fMass;
		 * vVelocity = vAvgVelocity + vAcceleration*fDelta/2.0f;
		 * 
		 * <0,0,0> = <0,0,0> + <0,0,0>*(200 ms or something)/2.0f;
		 * <0,0,0> += <0,0,0> * 200 ms or something;
		 * <X,Y,Z accel read in (sensor changed value stored in temp array list vector?)>;
		 * vVelocity = <0,0,0> + <X,Y,Z accels>*200 ms or something/2.0f;		
		 */
		if(isChanged && accelerometerValues != null){

			//Line 2
			for (int h=0; h<3; h++){
				vPosition[h] += vAvgVelocity[h] * TIME_STEP;
			}
			Log.v("POS", vPosition[0] + " " + vPosition[1] + " " + vPosition[2]);
			
			//Line 1
			//Ignores values that could be a result of sensor inconsistencies
			for (int i=0; i<3; i++){
				if (vAcceleration[i] < 0.05 && vAcceleration[i] > -0.05)
					vAvgVelocity[i] = vVelocity[i];
				vAvgVelocity[i] = vVelocity[i] + vAcceleration[i] * TIME_STEP / 2.0f;
			}
			Log.v("AVG", vAvgVelocity[0] + " " + vAvgVelocity[1] + " " + vAvgVelocity[2]);
			
			//Line 3
			//I'm a bit worried here about concurrency issues, as the values to accelerometerValues could potentially change again. Guess we'll find out soon!
			for (int j=0; j<3; j++){
//				Log.v("VEC" + j, accelerometerVector.get(j) + "");
//				Log.v("VAL" + j, accelerometerValues[j] + "");
//				vAcceleration[j] = accelerometerVector.get(j).floatValue() - calibrationOffset[j];
//				vAcceleration[j] = accelerometerValues[j] - calibrationOffset[j];
				vAcceleration[j] = accelerometerValues[j];
			}
			Log.v("ACE", vAcceleration[0] + " " + vAcceleration[1] + " " + vAcceleration[2]);
//			Log.v("ACU", accelerometerVector.get(0).floatValue() + " " + accelerometerVector.get(1).floatValue() + " " + accelerometerVector.get(2).floatValue());	// accel untouched
			Log.v("ACU", accelerometerValues[0] + " " + accelerometerValues[1] + " " + accelerometerValues[2]);	// accel untouched
//			Log.v("CAL", calibrationOffset[0] + " " + calibrationOffset[1] + " " + calibrationOffset[2]);		// calibration offset
			
			//Line 4
			for (int k=0; k<3; k++){
				if(vAcceleration[k] < 0.05 && vAcceleration[k] > -0.05)
					vVelocity [k] = vAvgVelocity[k];
				vVelocity[k] = vAvgVelocity[k] + vAcceleration[k] * TIME_STEP / 2.0f;
			}
			Log.v("VEL", vVelocity[0] + " " + vVelocity[1] + " " + vVelocity[2]);
			
			
			
		}
		isChanged = false;
	}

	protected void onStop() {
		super.onStop();
		sensorManager.unregisterListener(this);
	}

	public void onAccuracyChanged(Sensor arg0, int arg1) {
		// Hitachi VCR and Shrimp Tempura
	}

	public void onSensorChanged(SensorEvent event) {
		/*
		 * TODO this method will have to be synchronized to avoid concurrency issues I saw some place 
		 * that had synchronize(this){ ... } wrapped around the inside of the entire onSensorChanged method
		 */
		synchronized(this){
			isChanged = true;

			if(enabled){
				switch (event.sensor.getType()) {
				case Sensor.TYPE_MAGNETIC_FIELD:
					geomagneticMatrix = event.values.clone();
					sensorReady = true;
					break;
				case Sensor.TYPE_ACCELEROMETER:
//					accelerometerVector = new Vector(Arrays.asList(event.values.clone()));
					accelerometerValues = event.values.clone();
				}   

				if (geomagneticMatrix != null && accelerometerValues != null && sensorReady) {
					sensorReady = false;


					floatXTotal += accelerometerValues[0];

					// Display
					tvXCoordinate.setText(vAcceleration[0] + "");
					tvYCoordinate.setText(vAcceleration[1] + "");
					tvZCoordinate.setText(vAcceleration[2] + "");
//					tvXTotal.setText(floatXTotal + "");
//					tvAzimuth.setText(orientationValues[0] + "");
//					tvPitch.setText(orientationValues[1] + "");
//					tvRoll.setText(orientationValues[2] + "");
					
					tvDisplace.setText(vPosition[0] + ", " + vPosition[1] + ", " + vPosition[2]);
					
					
					SensorManager.getRotationMatrix(rotationMatrix, inclinationMatrix, accelerometerValues, geomagneticMatrix);
//					SensorManager.getOrientation(rotationMatrix, orientationValues);
						
				}
			}
			/*
			// same but for calibration offset, doesn't have display stuff, or timing stuff.
			else if (!enabled){
				switch (event.sensor.getType()) {
				case Sensor.TYPE_MAGNETIC_FIELD:
					geomagneticMatrix = event.values.clone();
					sensorReady = true;
					break;
				case Sensor.TYPE_ACCELEROMETER:
					calibrationOffset = event.values.clone();
				}   

				if (geomagneticMatrix != null && calibrationOffset != null) {
					sensorReady = false;
					SensorManager.getRotationMatrix(rotationMatrix, inclinationMatrix, calibrationOffset, geomagneticMatrix);
					SensorManager.getOrientation(rotationMatrix, orientationValues);
				}
			}
			*/
			
			// test this out hard core Aaron
//			switch (event.sensor.getType()){
//			case Sensor.TYPE_MAGNETIC_FIELD:
//				geomagneticMatrix = event.values.clone();
//				sensorReady = true;
//				break;
//			case Sensor.TYPE_ACCELEROMETER:
//				if (enabled)
//					accelerometerValues = event.values.clone();
//				else
//					calibrationOffset = event.values.clone();
//			}
//			
//			if (geomagneticMatrix != null && (calibrationOffset != null || accelerometerValues !=null)) {
//				sensorReady = false;
//				if (enabled)
//					SensorManager.getRotationMatrix(rotationMatrix, inclinationMatrix, accelerometerValues, geomagneticMatrix);
//				else
//					SensorManager.getRotationMatrix(rotationMatrix, inclinationMatrix, calibrationOffset, geomagneticMatrix);
//				SensorManager.getOrientation(rotationMatrix, orientationValues);
//			}
			
		}
	}

}